#include <stdint.h>
//#include "freeRTOS.h"


#define DELAY_ON_ENTRY_TO_AUTO_MODE   10         // time after releasing FSR before auto mode kicks in (defined in 10ths of a second)

#define BUFFER_LENGTH       4

#define  NEW_DIFF_BUFF_LEN 10

#define SPEED_BUFF_LENGTH (unsigned int)15


const int TICKS_PER_REVOLUTION = 1.75e3f;//1750;    //90/26*360*2=2500;  90/37*360*2=1750  // number of ticks per revolution of the wheel  both rising and falling edge
const float WHEEL_RADIUS = 3.8f;           // radius of the wheel (in inches)


// CountingTask helper functions
void CT_pushMode_updateSpeedSetpoint(void);
void CT_pushMode_initPushSequence(void);
void CT_pushMode_initPushInPushSequence(void);
void CT_pushMode_checkForFirstPush(void);
void CT_pushMode_respondToPush(void);
void CT_pushMode_main(void);
void CT_pushMode_disable(void);

void CT_kinematic_init(void);
void CT_kinematic_filter_prep(void);
void CT_kinematic_filter_update(void);
float CT_butterworth_filter_update(unsigned int x_val);
float CT_average_filter_update(unsigned int x_val);
void CT_get_encoder_SpeedAndDirection(void);

/*inline*/ bool CT_getLowerTransient(void);
/*inline*/ bool CT_pushMode_isMaxSpeedReached(void);
/*inline*/ bool CT_pushMode_isFiltAccelUnderMaxThresh(void);
/*inline*/ bool CT_pushMode_isFiltAccelOverMinThresh(void);
/*inline*/ bool CT_pushMode_isMaxAccelUnderThresh(void);
/*inline*/ bool CT_pushMode_isAccelIncreasing(void);
/*inline*/ bool CT_pushMode_pushTooLongLikelyDownhill(void);

void CT_pushMode_respondToFiltAccelTooHigh(void);
void CT_pushMode_resetPushInPushDetection(void);

/*inline*/ void CT_setNewSpeedSetpoint(float setPoint);
/*inline*/ void CT_resetNewSpeedSetpoint(void);
void CT_pushMode_updateNewSpeedSetpoint(float curr_speed);
bool CT_pushMode_isFirstPushValid(void);
/*inline*/ float CT_pushMode_getNewSetPoint(void);
/*inline*/ void CT_set_speed (uint32_t freq, float _spd);
/*inline*/ float CT_get_speed (void);
/*inline*/ void CT_set_filter_speed (uint32_t freq, float _spd);
/*inline*/ float CT_get_filter_speed (void);
/*inline*/ void CT_set_cap_val (signed int _cap_val);
/*inline*/ signed int CT_get_cap_val (void);
/*inline*/ void CT_set_prev_cap_val (unsigned int _cap_val);
/*inline*/ bool CT_getPushInPush(void);
/*inline*/ void CT_setPushInPush(bool val);
/*inline*/ unsigned int CT_get_prev_cap_val (void);
/*inline*/ void set_direction_reverse (bool _dir);
/*inline*/ bool get_direction_reverse (void);
/*inline*/ void CT_resetSpeedSetpoint(void);
/*inline*/ void CT_setSpeedSetpoint(float val);
/*inline*/ float CT_getSpeedSetpoint(void);
/*inline*/ void CT_setPushDetected(bool val);
/*inline*/ signed int CT_getLastAccel(void);
/*inline*/ unsigned int CT_getPushDetectedDelay(void);
/*inline*/ void CT_resetPushDetectedDelay();
/*inline*/ void CT_resetPushStartDelay();
/*inline*/ void CT_resetDelayAfterManual(void);
/*inline*/ void CT_setLowerTransient(bool val);
/*inline*/ bool CT_getLowerTransient(void);
/*inline*/ void CT_setLowerTransientWheelie(bool val);
/*inline*/ bool CT_getLowerTransientWheelie(void);
/*inline*/ bool CT_pushMode_isAFirstPush(void);



//inline void CT_setPushDetected(bool val)
//{
//  CT_push_detected = val;
//}
//inline bool CT_getPushDetected(void)
//{
//  return CT_push_detected;
//}
//inline void CT_resetDelayAfterManual(void)
//{
//  CT_delay_after_manual_mode = 0;
//}
//inline void CT_resetPushStartDelay()
//{
//  CT_push_start_delay = 0;
//}
//inline void CT_resetPushDetectedDelay()
//{
//  CT_push_detected_delay = 0;
//}
//inline unsigned int CT_getPushDetectedDelay(void)
//{
//  return CT_push_detected_delay;
//}
//inline void CT_set_speed (uint32_t freq, float _spd)
//{
////  sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);
//  CT_speed = ( (float)_spd  * ((float)freq) * (WHEEL_RADIUS) ) / ( TICKS_PER_REVOLUTION*((float)2.801127) ); 
//}
//inline float CT_get_speed (void)
//{
//  return CT_speed;
//}
//inline void CT_set_filter_speed (uint32_t freq, float _spd)
//{
////  sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);
//  CT_filter_speed = ( (float)_spd  * ((float)freq) * (WHEEL_RADIUS) ) / ( TICKS_PER_REVOLUTION*((float)2.801127) ); 
//}
//inline float CT_get_filter_speed (void)
//{
//  return CT_filter_speed;
//}
//inline void CT_set_cap_val (signed int _cap_val)
//{
//  CT_cap_val = _cap_val; 
//}
//inline signed int CT_get_cap_val (void)
//{
//  return CT_cap_val;
//}
//inline void CT_set_prev_cap_val (unsigned int _cap_val)
//{
//  CT_prev_cap_val = _cap_val; 
//}
//inline unsigned int CT_get_prev_cap_val (void)
//{
//  return CT_prev_cap_val;
//}
//inline void set_direction_reverse (bool _dir)
//{
//  direction_reverse = _dir; 
//}
//inline bool get_direction_reverse (void)
//{
//  return direction_reverse;
//}
//inline bool CT_pushMode_isFiltAccelUnderMaxThresh(void)
//{
//  return (filtered_vals[BUFFER_LENGTH -1] < 300);
//}
//inline bool CT_pushMode_isFiltAccelOverMinThresh(void)
//{
//  return (filtered_vals[BUFFER_LENGTH -1] > 16);
//}
//inline bool CT_getPushInPush(void)
//{
//  return CT_pushInPush;
//}
//inline void CT_setPushInPush(bool val)
//{
//  CT_pushInPush = val;
//}
//inline bool CT_pushMode_isAccelIncreasing(void)
//{
//  return (filtered_DD[BUFFER_LENGTH -1] > 0);
//}
//inline bool CT_pushMode_isAFirstPush(void)
//{
//  return ((CT_pushMode_isFiltAccelOverMinThresh() == true) && (CT_pushMode_isFiltAccelUnderMaxThresh() == true) && (CT_pushMode_isAccelIncreasing() == true));
//}
//inline void CT_pushMode_setNewSpeedSetpoint(float setPoint)
//{
//    CT_newSpeedSetoint = setPoint;
//}
//inline void CT_resetSpeedSetpoint(void)
//{
//  CT_speedSetpoint = 0;
//}
//inline void CT_setSpeedSetpoint(float val)
//{
//  CT_speedSetpoint = val;
//}
//inline float CT_getSpeedSetpoint(void)
//{
//  return CT_speedSetpoint;
//}
//inline float CT_pushMode_getNewSetPoint(void)
//{
//  return CT_newSpeedSetoint;
//}
//inline bool CT_pushMode_pushTooLongLikelyDownhill(void)
//{
//  // pushing time is too long, not a push; this means that we detected a push that lasted longer than 1 second, which
//  // is interpreted as rolling down a hill and picking up velocity. A push lasts 500 to 600 ms, so we should see a 
//  // decrease in speed after 5 or 6 CT_push_start_delay ticks.
//  return (CT_push_start_delay>10);
//}
//inline bool CT_pushMode_isMaxSpeedReached(void)
//{
//  return (CT_get_speed() < CT_pushMode_getNewSetPoint());
//}
//inline void CT_setLowerTransient(bool val)
//{
//  CT_lowerTransientEvent = val;
//}
//inline bool CT_getLowerTransient(void)
//{
//  return (CT_lowerTransientEvent);//>>4);
//}
//inline void CT_setLowerTransientWheelie(bool val)
//{
//  CT_lowerTransientWheelieEvent = val;
//}
//inline bool CT_getLowerTransientWheelie(void)
//{
//  return (CT_lowerTransientWheelieEvent);//>>4);
//}
//inline signed int CT_getLastAccel(void)
//{
//  return diff[BUFFER_LENGTH-1];
//}
